#ifndef __IMAGE_UNDISTORTER_H
#define __IMAGE_UNDISTORTER_H

#include <TooN/TooN.h>
#include "ATANCamera.h"
#include <cvd/image.h>
#include <cvd/rgb.h>
#include <cvd/internal/pixel_operations.h>
#include <cvd/internal/rgb_components.h>
#include <cvd/vector_image_ref.h>

using namespace TooN;

class ImageUndistorter 
{
 public:
  ImageUndistorter();
  ~ImageUndistorter();
  const CVD::BasicImage<CVD::Rgb<CVD::byte> >& Undistort(const CVD::BasicImage<CVD::Rgb<CVD::byte> > &imDistortedRGB);
  template<class T> const T BilinearPixel(const CVD::BasicImage<T> &imDistortedRGB, const Vector<2> &v2Position)
  {
	  TooN::Vector<2> delta =  v2Position - floor(v2Position);
	  CVD::ImageRef p = CVD::ir(floor(v2Position));

	  double x = delta[0];
	  double y = delta[1];

	  T pixel;

	  for(unsigned int i=0; i < CVD::Pixel::Component<T>::count; i++)
	  {
		  float a=0, b=0, c=0, d=0;

      if(imDistortedRGB.in_image(p))
        a = CVD::Pixel::Component<T>::get(imDistortedRGB[p + CVD::ImageRef(0,0)], i) * (1-x) * (1-y);

		  if(x != 0 && imDistortedRGB.in_image(p + CVD::ImageRef(1,0)))
			  b = CVD::Pixel::Component<T>::get(imDistortedRGB[p + CVD::ImageRef(1,0)], i) * x * (1-y);
		
		  if(y != 0 && imDistortedRGB.in_image(p + CVD::ImageRef(0,1)))
		  c = CVD::Pixel::Component<T>::get(imDistortedRGB[p + CVD::ImageRef(0,1)], i) * (1-x) * y;

		  if(x !=0 && y != 0 && imDistortedRGB.in_image(p + CVD::ImageRef(1,1)))
			  d = CVD::Pixel::Component<T>::get(imDistortedRGB[p + CVD::ImageRef(1,1)], i) * x * y;
		
		  CVD::Pixel::Component<T>::get(pixel, i) = a + b + c + d;
	  };

    return pixel;
  }

 protected:
  ATANCamera *mpCamera;
  CVD::ImageRef mirUndistortSize;
  CVD::Image<TooN::Vector<2> > mimUndistortMap;
  CVD::Image<CVD::Rgb<CVD::byte> > mimUndistortedRGB;

	inline TooN::Vector<2> floor(const TooN::Vector<2>& v) { return TooN::makeVector( ::floor(v[0]), ::floor(v[1])); };
	inline TooN::Vector<2> ceil(const TooN::Vector<2>& v) { return TooN::makeVector( ::ceil(v[0]), ::ceil(v[1])); };
  inline bool in_image(const TooN::Vector<2>& pos) { return mimUndistortedRGB.in_image(CVD::ir(floor(pos))) && mimUndistortedRGB.in_image(CVD::ir(ceil(pos))); };
};

#endif

